package Utilidades;

public class CalculadoraDeMisiles {

	/**
	 * @brief Constructor predeterminado de la clase CalculadoraDeMisiles
	 */
	public CalculadoraDeMisiles(){}
	
	
	/**
	 * @brief Funcion que determina el punto de ingreso de un misil enemigo a la zona 
	 * 		  protegida a partir de dos posiciones dadas de dicho misil
	 * @param p1 primera posicion obtenida del misil enemigo
	 * @param p2 segunda posicion obtenida del misil enemigo
	 * @return posicion del punto de entrada del misil enemigo a la zona proegida.
	 */
	public double[] puntoDeIngresoAZonaProtegida(double[] p1,double[] p2){
		//Defino un valor de error para devolver en caso de ser necesario
		double[] error=new double[3];
		error[0]=50000;error[1]=50000;error[2]=50000;

				
		//Calculo si pega o no en el cilindro
		long R=10000;
		double[] parametros=calcularParametros(p1,p2,R);
		if(checkNaN(parametros[1])){
			//System.out.println("Este misil no pega en el area protegida");
			return error;
		}
				
		//Calculo el indice del parametro que determina el punto de ingreso
		int i = indexParametroUtil(p1,p2,parametros);
		if(i==2){
			//System.out.println("Este misil no pega en el area protegida");
			return error;
		}
		
		//Calculo Px,Py,Pz y el correspondiente valor de R pra verificar que este bien.
		double Px=p2[0]+parametros[i]*(p2[0]-p1[0]);
		double Py=p2[1]+parametros[i]*(p2[1]-p1[1]);
		double Pz=p2[2]+parametros[i]*(p2[2]-p1[2]);
		//double newR=redondear(Math.sqrt(Px*Px+Py*Py),2);
			
		double[] posicion = double2LongArray(Px,Py,Pz);
		/*System.out.println("El Enemigo ingresa por el punto: ("+posicion[0]+
				";"+posicion[1]+";"+posicion[2]+")  R="+newR);*/
		
		return posicion;
	}


	
	

	/**
	 * @brief Funcion que devuelve verdadero si el parametro es NaN
	 * 		  (Not-a-Number). Devuelve falso en caso contrario.
	 * @param numero	Valor a checkear.
	 * @return {@code true} Si el parametro es NaN.
	 *         {@code false} Si el parametro no es NaN.
	 */
	private boolean checkNaN(double numero){
		return (numero!=numero);
	}
	
	/**
	 * @brief Funcion que redondea un double determinada cantidad de valores despues de la coma.
	 * @param numero valor double a redondear
	 * @param decimales cantidad de decimales que se desea tener
	 * @return devuelve el parametro "numero" redondeado con "decimales" decimales.
	 */
	public static double redondear(double numero,int decimales){
		double a=Math.pow(10,decimales);
		return Math.rint(numero*a)/a;
	}
	
	/**
	 * @brief Funcion que toma 3 numeros double y arma un arreglo long de 3 elementos con ellos
	 * @param px
	 * @param py
	 * @param pz
	 * @return arreglo long de 3 elementos
	 */
	private double[] double2LongArray(double px,double py,double pz){
		double[] resultado = new double[3];
		resultado[0]=(double) redondear(px,0);
		resultado[1]=(double) redondear(py,0);
		resultado[2]=(double) redondear(pz,0);
		return resultado;
	}
	
	/**
	 * @brief Funcion que calcula parametros necesarios para conocer el punto donde
	 * 		  un misil enemigo prga en el cilindro que define el perimetro del area
	 * 	  	  protegida. Requiere dos posiciones del misil enemigo tomadas en instantes
	 * 		  diferentes.
	 * 		  La logica de la funcion es la siguiente:
	 * 		  p1={x1,y1,z1} y p2={x2,y2,z2}
	 * 		  La recta que define la trayectoria del misil enemigo es: 
	 * 			P=p2+t*(p1-p2)	o bien:		X=x2+t*(x1-x2)
	 * 										Y=y2+t*(y1-y2)
	 * 										Z=z2+t*(z1-z2)
	 * 		  El circulo que define el perimetro del area protegida es: 
	 * 			x^2+y^2=10000^2
	 * 		  Luego, el misil pega en algun lado del cilindro si existe un valor "t" tal que:
	 * 			[x2+t*(x1-x2)]^2+[y2+t*(y1-y2)]^2=10000^2
	 * 		  Esto resulta en una eciacion cuadratica cuya solucion son los valores de t que
	 * 		  corresponden al punto de entrada al circulo y al punto de salida. La funcion
	 * 		  devuelve ambos valores de t en un arreglo double.
	 * 		  Para determinar cual de los dos valores es el de entrada, se debe de ver cual de
	 * 		  ellos da un valor Z tal que Z=z2+t*(z1-z2)>0. Esto se debe a que el misil no sale
	 * 		  del area protegida sino que impacta dentro de ella, lo cual sucede cuando z=0.
	 * @param p1 primera posicion obtenida del misil enemigo
	 * @param p2 segunda posicion obtenida del misil enemigo
	 * @param R Radio del circulo que define el area protegia
	 * @return arreglo con los dos parametros t posibles.
	 */
	public double[] calcularParametros(double[] p1,double[]p2,long R){
		//long R=10000;
		if((p1.length!=3)||(p2.length!=3))
			System.err.println("Error en dimensiones de parametros cuadraticos.");
				
		//System.out.println("p1: "+p1[0]+";"+p1[1]+";"+p1[2]);
		//System.out.println("p2: "+p2[0]+";"+p2[1]+";"+p2[2]);
		double a=(double)((p2[0]-p1[0])*(p2[0]-p1[0])+(p2[1]-p1[1])*(p2[1]-p1[1]));
		double b=(double)(2*p2[0]*(p2[0]-p1[0])+2*p2[1]*(p2[1]-p1[1]));
		double c=(double)(p2[0]*p2[0]+p2[1]*p2[1]-(R*R));
		//System.out.println("a="+a+"  b="+b+"  c="+c);
				
		double X1=(double)(-b+Math.sqrt(b*b-4*a*c))/(2*a);
		double X2=(double)(-b-Math.sqrt(b*b-4*a*c))/(2*a);

		//System.out.println("X1="+X1+"  X2="+X2);
		double parametros[]=new double[2];
		parametros[0]=X1;
		parametros[1]=X2;
		
		return parametros;
	}
	
	/**
	 * @brief Funcion que determina el indice del parametro del arreglo "parametros" que
	 * 		  hace que Pz sea un valor positivo. Si ambos indices cumplen con esto retorna
	 * 		  un indice i=2, que producira una Excepcion IndexOutOfBoundException, indicando
	 * 		  Que el misil no golpea el suelo dentro del area protegida.
	 * @param p1 primera posicion obtenida del misil enemigo
	 * @param p2 segunda posicion obtenida del misil enemigo
	 * @param parametros arreglo de parametros obtenido de la funcion calcularParametros
	 * @return {@code 0} Si el indice 0 corresponde al parametro correcto.
	 *         {@code 1} Si el indice 0 corresponde al parametro correcto.
	 *         {@code 2} Si ambos parametros son incorrectos.
	 */
	private int indexParametroUtil(double[] p1,double[] p2,double[] parametros){
		double Pz1=p2[2]+parametros[0]*(p2[2]-p1[2]);
		double Pz2=p2[2]+parametros[1]*(p2[2]-p1[2]);

		//System.out.println("PZ1="+redondear(Pz1,2)+"   PZ2="+redondear(Pz2,2));
		int i=2;
		if((Pz1>0&&Pz2>0)||(Pz1<0&&Pz2<0))
			{i=2;} 
		else if(Pz1>0&&Pz2<0) 
			{i=0;}
		else {i=1;}
		return i;
	}
	
	
	/**
	 * @brief Esta funcion calcula el vector de velocidad de un misil aliado en etapa de posicionamiento.
	 * @param P1 posicion actual del misil
	 * @param P2 posicion a la cual se debe llegar
	 * @param velocidad del misil (casi siempre es 100)
	 * @return nuevo vector de velocidad
	 */
	public double[] calcularNuevoVectorDeVelocidad(double[] p1, double[] p2,int velocidad){

		//Primero, obtengo el vector director del vector velocidad, el cual viene con un modulo cualquiera
		double []vectorVelocidad = new double[3];
		vectorVelocidad[0]=(long) (p2[0]-p1[0]);
		vectorVelocidad[1]=(long) (p2[1]-p1[1]);
		vectorVelocidad[2]=(long) (p2[2]-p1[2]);
		//Luego calculo el modulo de este vector director:
		double modulo1=Math.sqrt(vectorVelocidad[0]*vectorVelocidad[0]+vectorVelocidad[1]*vectorVelocidad[1]+
				vectorVelocidad[2]*vectorVelocidad[2]);
		//Despues calculo un valor tal que haga que al multiplicar las componntes del vector por dicho valor
		//el modulo del vector director sea equivalente a la velocidad del misil
		double T=(double) (velocidad/modulo1);
		vectorVelocidad[0]= T*(p2[0]-p1[0]);
		vectorVelocidad[1]= T*(p2[1]-p1[1]);
		vectorVelocidad[2]= T*(p2[2]-p1[2]);
		
		//El nuevo modulo debera ser 100:
		printVector(vectorVelocidad,2);
		
		//Devuelvo el vector velocidad obtenido
		return vectorVelocidad;
	}
	
	/**
	 * @brief Funcion que imprime un vector en pantalla con su modulo
	 * @param vector
	 * @param redondeo
	 */
	public void printVector(double[] vector,int redondeo){
		double modulo = Math.sqrt(vector[0]*vector[0]+vector[1]*vector[1]+vector[2]*vector[2]);
		modulo=redondear(modulo,redondeo);
		System.out.print("["+redondear(vector[0],redondeo)+";"+redondear(vector[1],redondeo)+";"+
				redondear(vector[2],redondeo)+"]");
		System.out.println("Modulo: "+modulo);
	}
	

}
